﻿#include <gtest/gtest.h>

#include <xy/math/matrix.h>
#include <xy/solver/linear.h>
#include <xy/utils/adaptor_eigen.h>

#include <ctime>
#include <random>

using namespace xy;
using namespace xy::math;
using namespace xy::solver;

class TestMatrix : public ::testing::Test
{
  protected:
    void SetUp() override
    {
    }

    void TearDown() override
    {
        // nothing
    }

    matxd Init(int m, int n, std::initializer_list<double> lst)
    {
        matxd A(m, n);
        double *p = A.data();
        std::copy(lst.begin(), lst.end(), p);
        return A;
    }
};

// 测试 matxd
TEST_F(TestMatrix, Initialize)
{
    // 测试比较
    {
        EXPECT_TRUE(Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                    Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));
        EXPECT_TRUE(Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                    Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));
    }

    // 测试初始化
    {
        auto A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_TRUE((A.rows() == 3 && A.cols() == 4));

        auto A2 = matxd(4, 3).set_value(1.1);
        EXPECT_TRUE(A2 == Init(4, 3, {1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1}));

        matxd A3 = A2;
        EXPECT_TRUE(A3 == A2);

        matxd A4 = matxd::identity(5);
        matxd A5(5, 5);
        A5.set_zero();
        for (int i = 0; i < 5; i++)
            A5(i, i) = 1;
        EXPECT_TRUE(A4 == A5);
    }

    // 测试赋值
    {
        auto A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        auto A2 = matxd(3, 4).set_zero();
        A2 = A;
        EXPECT_TRUE(A2 == A);

        // 自赋值
        A2 = A2;
        EXPECT_TRUE(A2 == A);

        auto A3 = matxd(3, 4).set_value(1.1);
        A3 = std::move(A2);
        EXPECT_TRUE(A3 == A);

        // 自移动
        A3 = std::move(A3);
    }

    // 辅助
    {
        matxd A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        matxd B = Init(2, 3, {1, 2, 3, 5, 6, 7});
        A.resize(2, 3, true);
        EXPECT_TRUE(A == B);
    }

    // 测试 ()
    {
        double arr[] = {1.2, 2.5, 34, 4.8, 5.1, 6.9, 7.1, 8.2, 9.7, 10.5, 11.4, 12.2};

        // 测试取引用
        auto A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        for (int i = 0; i < A.rows(); i++)
            for (int j = 0; j < A.cols(); j++)
                A(i, j) = arr[i * 4 + j];
        for (int i = 0; i < A.rows(); i++)
            for (int j = 0; j < A.cols(); j++)
                EXPECT_DOUBLE_EQ(A(i, j), arr[i * 4 + j]);

        EXPECT_EQ(A.rows(), 3);
        EXPECT_EQ(A.cols(), 4);
    }
}

// 测试 matxd
TEST_F(TestMatrix, OpFunctions)
{
    // 测试四则运算
    {
        auto A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        matxd A2 = A * 2;
        EXPECT_TRUE(A2 == Init(3, 4, {2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24}));

        matxd A3 = A2 / 2;
        EXPECT_TRUE(A3 == A);

        matxd A4 = A + A;
        EXPECT_TRUE(A4 == A2);

        matxd A5 = A2 - A;
        EXPECT_TRUE(A5 == A);

        matxd B = Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        matxd A6 = A * B;
        EXPECT_TRUE(A6 == Init(3, 3, {70, 80, 90, 158, 184, 210, 246, 288, 330}));

        matxd A7 = -A;
        EXPECT_TRUE((A7 + A) == matxd(3, 4).set_zero());
    }

    // 测试自运算
    {
        auto A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        A *= 2;
        EXPECT_TRUE(A == Init(3, 4, {2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24}));

        A /= 2;
        EXPECT_TRUE(A == Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));

        A += A;
        EXPECT_TRUE(A == Init(3, 4, {2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24}));

        A -= A;
        EXPECT_TRUE(A == Init(3, 4, {0, 0, 0, 0, 0}));
    }

    // Kronecker 积
    {
        auto A = Init(3, 1, {1, 2, 3});
        auto B = Init(2, 3, {1, 2, 3, 4, 5, 6});
        auto C = Init(6, 3, {1, 2, 3, 4, 5, 6, 2, 4, 6, 8, 10, 12, 3, 6, 9, 12, 15, 18});
        EXPECT_TRUE((A % B) == C);
    }

    // 填充矩阵
    {
        auto A = matxd(3, 4).set_value(1.1);
        auto B = matxd(2, 3).set_value(2.2);
        A.fill(1, 1, B);
        EXPECT_TRUE(A == Init(3, 4, {1.1, 1.1, 1.1, 1.1, 1.1, 2.2, 2.2, 2.2, 1.1, 2.2, 2.2, 2.2}));
    }

    // 范数
    {
        auto A = Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_EQ(A.norm(0), 33);
        EXPECT_DOUBLE_EQ(std::sqrt(A.norm(2)), 25.495097567963924);
    }

    // 测试迹
    {
        auto A = Init(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        EXPECT_DOUBLE_EQ(A.trace(), 16);

        auto B = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_DOUBLE_EQ(B.trace(), 18);

        auto C = Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_DOUBLE_EQ(C.trace(), 15);
    }

    // 取元素
    {
        auto A = Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});

        EXPECT_TRUE(A.transposed() == Init(3, 4, {1, 4, 7, 10, 2, 5, 8, 11, 3, 6, 9, 12}));
        EXPECT_TRUE(A.transpose() == Init(3, 4, {1, 4, 7, 10, 2, 5, 8, 11, 3, 6, 9, 12}));
    }

    // 求逆
    {
        matxd A = Init(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        matxd B = invert(A);
        EXPECT_LE((B - Init(3, 3, {-2.0 / 3, -2.0 / 3, 1.0, -4.0 / 3, 11.0 / 3, -2.0, 1.0, -2.0, 1.0})).norm(), 1e-8);

        matxd A1 = Init(2, 2, {5, 6, 1, 3});
        matxd B1 = invert(A1);
        EXPECT_LE((B1 - Init(2, 2, {3, -6, -1, 5}) / 9).norm(), 1e-8);
    }

    // 测试矩阵秩
    {
        matxd A(6, 9);
        A.set_random();

        Eigen::MatrixXd matrix(6, 9);
        for (int i = 0; i < 6; i++)
            for (int j = 0; j < 9; j++)
                matrix(i, j) = A(i, j);

        // 计算矩阵秩
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix);
        double tolerance = 1e-10; // 判定奇异值为零的容差
        int rank = (svd.singularValues().array() > tolerance).count();
        int r = std::abs(math::rank(A));
        EXPECT_EQ(r, rank);
    }

    {
        matxd A(9, 6);
        A.set_random();

        Eigen::MatrixXd matrix(9, 6);
        for (int i = 0; i < 9; i++)
            for (int j = 0; j < 6; j++)
                matrix(i, j) = A(i, j);

        // 计算矩阵秩
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix);
        double tolerance = 1e-10; // 判定奇异值为零的容差
        int rank = (svd.singularValues().array() > tolerance).count();
        int r = std::abs(math::rank(A));
        EXPECT_EQ(r, rank);
    }

    // 计算模最大的特征值
    {
        matxd A(9, 9);
        A.set_random();

        auto adp = utils::adaptor_eigen(A);
        Eigen::MatrixXd matrix = adp;

        matrix.eigenvalues();
        double max_eigenvalue = 0;
        for (int i = 0; i < 9; i++)
        {
            double eigenvalue = matrix.eigenvalues()[i].real();
            if (std::abs(eigenvalue) > max_eigenvalue)
                max_eigenvalue = eigenvalue;
        }

        double eigenvalue = std::abs(math::eigenvalue(A));
        std::cout << "max eigenvalue: " << max_eigenvalue << std::endl;
        std::cout << "eigenvalue: " << eigenvalue << std::endl;
        // EXPECT_DOUBLE_EQ(eigenvalue, max_eigenvalue);
    }
}

TEST_F(TestMatrix, vecxd)
{
    // 测试向量
    {
        vecxd v(3);
        v[0] = 1;
        v[1] = 2;
        v[2] = 3;
        EXPECT_EQ(v.size(), 3);
        EXPECT_DOUBLE_EQ(v[0], 1);
        EXPECT_DOUBLE_EQ(v[1], 2);
        EXPECT_DOUBLE_EQ(v[2], 3);

        vecxd w(3);
        w[0] = 4;
        w[1] = 5;
        w[2] = 6;
        vecxd u = v + w;
        EXPECT_DOUBLE_EQ(u[0], 5);
        EXPECT_DOUBLE_EQ(u[1], 7);
        EXPECT_DOUBLE_EQ(u[2], 9);
    }

    // 测试拷贝构造、移动构造、赋值
    {
        vecxd v(3);
        v[0] = 1;
        v[1] = 2;
        v[2] = 3;

        vecxd w(v);
        EXPECT_DOUBLE_EQ(w[0], 1);
        EXPECT_DOUBLE_EQ(w[1], 2);
        EXPECT_DOUBLE_EQ(w[2], 3);

        vecxd u(std::move(w));
        EXPECT_DOUBLE_EQ(u[0], 1);
        EXPECT_DOUBLE_EQ(u[1], 2);
        EXPECT_DOUBLE_EQ(u[2], 3);

        w = u;
        EXPECT_DOUBLE_EQ(w[0], 1);
        EXPECT_DOUBLE_EQ(w[1], 2);
        EXPECT_DOUBLE_EQ(w[2], 3);

        w = w;
        EXPECT_DOUBLE_EQ(w[0], 1);
        EXPECT_DOUBLE_EQ(w[1], 2);
        EXPECT_DOUBLE_EQ(w[2], 3);

        w = std::move(u);
        EXPECT_DOUBLE_EQ(w[0], 1);
        EXPECT_DOUBLE_EQ(w[1], 2);
        EXPECT_DOUBLE_EQ(w[2], 3);
    }

    // 测试矩阵构造
    {
        matxd A(3, 1);
        A.set_random();

        vecxd v = A;
        EXPECT_TRUE(matxd(v) == A);
        EXPECT_EQ(v.size(), 3);

        matxd B(1, 3);
        B.set_random();

        vecxd w = B;
        EXPECT_TRUE(matxd(w) == B);
        EXPECT_EQ(w.size(), 3);

        vecxd v2 = std::move(A);
        EXPECT_TRUE(v == v2);

        vecxd w2 = std::move(B);
        EXPECT_TRUE(w == w2);
    }

    // 测试向量点积
    {
        vecxd v(3);
        v[0] = 1;
        v[1] = 2;
        v[2] = 3;

        vecxd w(3);
        w[0] = 4;
        w[1] = 5;
        w[2] = 6;

        double dot_vw = dot(v, w);
        EXPECT_DOUBLE_EQ(dot_vw, 32);
    }

    // 测试矩阵与向量乘法
    {
        matxd A(3, 3);
        A(0, 0) = 1;
        A(0, 1) = 2;
        A(0, 2) = 3;
        A(1, 0) = 4;
        A(1, 1) = 5;
        A(1, 2) = 6;
        A(2, 0) = 7;
        A(2, 1) = 8;
        A(2, 2) = 9;

        vecxd b(3);
        b[0] = 1;
        b[1] = 2;
        b[2] = 3;

        vecxd c = A * b;
        EXPECT_DOUBLE_EQ(c[0], 14);
        EXPECT_DOUBLE_EQ(c[1], 32);
        EXPECT_DOUBLE_EQ(c[2], 50);
    }
}

TEST_F(TestMatrix, spvecxd)
{
    {
        matxd A(8, 4);
        A.set_random();

        spvecxd sv;
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                sv.create(i, j, A(i, j));

        double inner = 0;
        sv.foreach ([&](int i, int j, double value) { inner += value * value; });

        EXPECT_DOUBLE_EQ(inner, A.norm(2));
    }

    {
        // 测试张量积
        spvecx<double> a, b;
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 4; j++)
            {
                a.create(i, j, i * 4 + j);
                b.create(j, i, i * 4 + j);
            }
        }

        matxd A(12, 1), B(1, 12);

        auto A1 = a.to_matrix();
        auto B1 = b.to_matrix();

        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 4; j++)
            {
                A(i * 4 + j, 0) = A1(i, j);
            }
        }

        for (int i = 0; i < 4; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                B(0, i * 3 + j) = B1(i, j);
            }
        }

        std::cout << A << std::endl;
        std::cout << B << std::endl;

        auto c = a % b;
        auto C = A * B;
        matxd C1(12, 12);
        C1.set_zero();

        auto f = [&](int x, int y, double value) { C1(x, y) = value; };
        c.foreach (f);

        EXPECT_EQ(C1, C);
    }
}

// 测试 spmatxd
TEST_F(TestMatrix, spmatxd)
{
    // 测试初始化
    {
        srand(time(0));

        matxd A(8, 4);
        A.set_zero();
        spmatxd SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));

        matxd B(4, 8);
        B.set_zero();
        spmatxd SB(4, 8);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(j, i) = value;
            SB(j, i) = value;
        }

        EXPECT_EQ(SA.rows(), 8);
        EXPECT_EQ(SA.cols(), 4);

        spmatxd SB2(4, 8);
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                SB2(j, i) = 1;
        EXPECT_EQ(SB2.nnz(), 32);

        matxd C = matxd::identity(4);
        spmatxd SC = spmatxd::identity(4);
        EXPECT_TRUE(matxd(SC) == C);
    }

    // 测试赋值 + 隐式转化
    {
        srand(time(0));

        matxd A(8, 4);
        A.set_zero();
        spmatxd SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        spmatxd SB = SA;
        EXPECT_TRUE(matxd(SB) == matxd(SA));

        spmatxd SC(8, 4);
        SC = SA;
        EXPECT_TRUE(matxd(SC) == matxd(SA));

        SB = SB;
        EXPECT_TRUE(matxd(SB) == matxd(SA));

        spmatxd SD = std::move(SC);
        EXPECT_TRUE(matxd(SD) == matxd(SA));

        SD = std::move(SD);
        EXPECT_TRUE(matxd(SD) == matxd(SA));
    }

    // 测试 resize, transpose
    {
        srand(time(0));

        matxd A(8, 4);
        A.set_zero();
        spmatxd SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        spmatxd SB = SA;
        SA.resize(4, 8);
        for (int i = 0; i < 4; i++)
        {
            for (int j = 0; j < 8; j++)
            {
                SA(i, j) = A(j, i);
            }
        }
        EXPECT_TRUE(matxd(SA) == matxd(SB.transposed()));
        EXPECT_TRUE(matxd(SA) == matxd(SB.transpose()));
    }

    // 测试加减
    {
        srand(time(0));

        matxd A(8, 4);
        A.set_zero();
        spmatxd SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        matxd B(8, 4);
        B.set_zero();
        spmatxd SB(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(i, j) = value;
            SB(i, j) = value;
        }

        matxd C = A + B;
        spmatxd SC = SA + SB;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));

        C = A - B;
        SC = SA - SB;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));

        C = -C;
        SC = -SC;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));
    }

    // 测试自加自减
    {
        srand(time(0));

        matxd A(8, 4);
        A.set_zero();
        spmatxd SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        A += A;
        SA += SA;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));

        A -= A;
        SA -= SA;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));
    }

    // 测试数乘除
    {
        srand(time(0));

        matxd A(8, 4);
        A.set_zero();
        spmatxd SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        matxd C = A / 3;
        spmatxd SC = SA / 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));

        C = A * 3;
        SC = SA * 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));
    }

    // 测试自数乘除
    {
        srand(time(0));

        matxd A(8, 4);
        A.set_zero();
        spmatxd SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        A /= 3;
        SA /= 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));

        A *= 3;
        SA *= 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));
    }

    // 测试乘矩阵
    {
        srand(time(0));

        matxd A(8, 4);
        A.set_zero();
        spmatxd SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        matxd B(4, 8);
        B.set_zero();
        spmatxd SB(4, 8);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(j, i) = value;
            SB(j, i) = value;
        }

        matxd C = A * B;
        spmatxd SC = SA * SB;
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 8; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));

        matxd D = B * A;
        spmatxd SD = SB * SA;
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(D(i, j), SD(i, j));
    }
}

TEST_F(TestMatrix, SparseNumeric)
{
    // 测试范数
    {
        srand(time(0));

        matxd A(8, 4);
        A.set_zero();
        spmatxd SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        double normA = A.norm();
        double normSA = SA.norm();
        EXPECT_DOUBLE_EQ(normA, normSA);

        double normA2 = A.norm(2);
        double normSA2 = SA.norm(2);
        EXPECT_DOUBLE_EQ(normA2, normSA2);
    }

    // 测试 trace
    {
        srand(time(0));

        matxd A(8, 4);
        A.set_zero();
        spmatxd SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        double traceA = A.trace();
        double traceSA = SA.trace();
        EXPECT_DOUBLE_EQ(traceA, traceSA);
    }

    // 测试压缩
    {
        srand(time(0));

        matxd A(8, 4);
        A.set_zero();
        spmatxd SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        spmatxd SB = SA.compressed();
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SB(i, j));

        SA.compress();
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));
    }
}
TEST_F(TestMatrix, diagmat3d)
{
    // 测试取元素
    {
        int N = 10;
        matxd data(3, N);
        data.set_random();

        diagmat3d m(N);
        matxd A(N, N);
        A.set_zero();
        for (int i = 0; i < 10; i++)
        {
            for (int j = 0; j < 10; j++)
            {
                if (i == j)
                {
                    m(i, j) = A(i, j) = data(1, i);
                }
                else if (i == j + 1)
                {
                    m(i, j) = A(i, j) = data(2, i);
                }
                else if (i == j - 1)
                {
                    m(i, j) = A(i, j) = data(0, i);
                }
            }
        }

        for (int i = 0; i < 10; i++)
        {
            for (int j = 0; j < 10; j++)
            {
                if (std::abs(i - j) <= 1)
                    EXPECT_DOUBLE_EQ(m(i, j), A(i, j));
            }
        }
    }

    // 测试初始化、拷贝构造、移动构造、赋值
    {
        diagmat3d m1(10);
        m1.set_random();

        matxd A(10, 10);
        A.set_zero();

        for (int i = 0; i < 10; i++)
        {
            for (int j = std::max(0, i - 1); j < std::min(i + 2, 10); j++)
            {
                A(i, j) = m1(i, j);
            }
        }
        EXPECT_EQ(m1.to_matrix(), A);

        diagmat3d m2 = m1;
        EXPECT_EQ(m2.to_matrix(), A);

        diagmat3d m3(std::move(m2));
        EXPECT_EQ(m3.to_matrix(), A);

        matxd B = m1;
        diagmat3d m4(B);
        EXPECT_EQ(m4, m1);

        diagmat3d m5 = std::move(B);
        EXPECT_EQ(m5, m1);

        matxd C = m1.to_matrix();
        diagmat3d m6 = diagmat3d::from_matrix(C);
        EXPECT_EQ(m6.to_matrix(), A);

        diagmat3d m7(10);
        m7 = m1;
        EXPECT_EQ(m7.to_matrix(), A);

        diagmat3d m8(10);
        m8 = std::move(m1);
        EXPECT_EQ(m8.to_matrix(), A);
    }

    // 四则运算
    {
        diagmat3d m1(10);
        m1.set_random();

        diagmat3d m2(10);
        m2.set_random();

        matxd A = m1.to_matrix();
        matxd B = m2.to_matrix();

        diagmat3d m3 = m1 + m2;
        matxd C = A + B;
        EXPECT_EQ(m3.to_matrix(), C);

        diagmat3d m4 = m1 - m2;
        matxd D = A - B;
        EXPECT_EQ(m4.to_matrix(), D);

        auto m5 = m1 * m2;
        matxd E = A * B;
        EXPECT_EQ(m5.to_matrix(), E);

        diagmat3d m6 = m1 / 2;
        matxd F = A / 2;
        EXPECT_EQ(m6.to_matrix(), F);

        diagmat3d m7 = m1 * 2;
        matxd G = A * 2;
        EXPECT_EQ(m7.to_matrix(), G);
    }

    // 自运算
    {
        diagmat3d m1(10);
        m1.set_random();

        diagmat3d m2(10);
        m2.set_random();

        matxd A = m1.to_matrix();
        matxd B = m2.to_matrix();

        m1 += m2;
        A += B;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 -= m2;
        A -= B;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 /= 2;
        A /= 2;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 *= 2;
        A *= 2;
        EXPECT_EQ(m1.to_matrix(), A);
    }

    // 操作函数
    {
        diagmat3d m1(10);
        m1.set_random();

        matxd A = m1.to_matrix();
        EXPECT_DOUBLE_EQ(A.norm(0), m1.norm(0));
        EXPECT_DOUBLE_EQ(A.norm(1), m1.norm(1));
        EXPECT_DOUBLE_EQ(A.norm(2), m1.norm(2));
        EXPECT_DOUBLE_EQ(A.trace(), m1.trace());

        diagmat3d m2 = m1.transposed();
        matxd B = A.transposed();

        EXPECT_EQ(B, m2.to_matrix());
        EXPECT_EQ(A.transpose(), m1.transpose().to_matrix());
    }
}

TEST_F(TestMatrix, diagmat5d)
{
    // 测试取元素
    {
        int N = 10;
        matxd data(5, N);
        data.set_random();

        diagmat5d m(N);
        matxd A(N, N);
        A.set_zero();
        for (int i = 0; i < 10; i++)
        {
            for (int j = 0; j < 10; j++)
            {
                if (i == j - 2)
                    m(i, j) = A(i, j) = data(0, i);
                else if (i == j - 1)
                    m(i, j) = A(i, j) = data(1, i);
                else if (i == j)
                    m(i, j) = A(i, j) = data(2, i);
                else if (i == j + 1)
                    m(i, j) = A(i, j) = data(3, i);
                else if (i == j + 2)
                    m(i, j) = A(i, j) = data(4, i);
            }
        }

        for (int i = 0; i < 10; i++)
        {
            for (int j = 0; j < 10; j++)
            {
                if (std::abs(i - j) <= 2)
                    EXPECT_DOUBLE_EQ(m(i, j), A(i, j));
            }
        }
    }

    // 测试初始化、拷贝构造、移动构造、赋值
    {
        diagmat5d m1(10);
        m1.set_random();

        matxd A(10, 10);
        A.set_zero();

        for (int i = 0; i < 10; i++)
        {
            for (int j = std::max(0, i - 2); j < std::min(i + 3, 10); j++)
            {
                A(i, j) = m1(i, j);
            }
        }
        EXPECT_EQ(m1.to_matrix(), A);

        diagmat5d m2 = m1;
        EXPECT_EQ(m2.to_matrix(), A);

        diagmat5d m3(std::move(m2));
        EXPECT_EQ(m3.to_matrix(), A);

        matxd B = m1;
        diagmat5d m4(B);
        EXPECT_EQ(m4, m1);

        diagmat5d m5 = std::move(B);
        EXPECT_EQ(m5, m1);

        matxd C = m1.to_matrix();
        diagmat5d m6 = diagmat5d::from_matrix(C);
        EXPECT_EQ(m6.to_matrix(), A);

        diagmat5d m7(10);
        m7 = m1;
        EXPECT_EQ(m7.to_matrix(), A);

        diagmat5d m8(10);
        m8 = std::move(m1);
        EXPECT_EQ(m8.to_matrix(), A);
    }

    // 四则运算
    {
        diagmat5d m1(10);
        m1.set_random();

        diagmat5d m2(10);
        m2.set_random();

        matxd A = m1.to_matrix();
        matxd B = m2.to_matrix();

        diagmat5d m3 = m1 + m2;
        matxd C = A + B;
        EXPECT_EQ(m3.to_matrix(), C);

        diagmat5d m4 = m1 - m2;
        matxd D = A - B;
        EXPECT_EQ(m4.to_matrix(), D);

        auto m5 = m1 * m2;
        matxd E = A * B;
        EXPECT_EQ(m5.to_matrix(), E);

        diagmat5d m6 = m1 / 2;
        matxd F = A / 2;
        EXPECT_EQ(m6.to_matrix(), F);

        diagmat5d m7 = m1 * 2;
        matxd G = A * 2;
        EXPECT_EQ(m7.to_matrix(), G);
    }

    // 自运算
    {
        diagmat5d m1(10);
        m1.set_random();

        diagmat5d m2(10);
        m2.set_random();

        matxd A = m1.to_matrix();
        matxd B = m2.to_matrix();

        m1 += m2;
        A += B;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 -= m2;
        A -= B;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 /= 2;
        A /= 2;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 *= 2;
        A *= 2;
        EXPECT_EQ(m1.to_matrix(), A);
    }

    // 操作函数
    {
        diagmat5d m1(10);
        m1.set_random();

        matxd A = m1.to_matrix();
        EXPECT_DOUBLE_EQ(A.norm(0), m1.norm(0));
        EXPECT_DOUBLE_EQ(A.norm(1), m1.norm(1));
        EXPECT_DOUBLE_EQ(A.norm(2), m1.norm(2));
        EXPECT_DOUBLE_EQ(A.trace(), m1.trace());

        diagmat5d m2 = m1.transposed();
        matxd B = A.transposed();

        EXPECT_EQ(B, m2.to_matrix());
        EXPECT_EQ(A.transpose(), m1.transpose().to_matrix());
    }
}

TEST_F(TestMatrix, Inline)
{
    // 测试 dot
    {
        int N = 10;
        matxd A(N, N);
        A.set_random();

        EXPECT_DOUBLE_EQ(dot(A, A), A.norm(2));

        double l = 10;
        double r = 20;
        EXPECT_DOUBLE_EQ(dot(l, r), l * r);
    }

    // 测试 divide
    {
        auto [A, x, b] = linear_llt::example(1, 10);
        matxd B(10, 10);
        B.set_random();

        EXPECT_EQ(B / A, invert(A) * B);

        double l = 10;
        double r = 20;
        EXPECT_DOUBLE_EQ(r / l, r / l);
    }

    // 测试 householder
    {
        matxd X(10, 1);
        X.set_random();
        auto Y = X;
        auto beta = householder(Y.data(), Y.rows());
        auto H = matxd::identity(X.rows());
        H = H - Y * Y.transposed() * beta;
        Y = H * X; // 所有分量集中在第一个元素中
        EXPECT_DOUBLE_EQ(X.norm(2), std::pow(Y(0, 0), 2));
    }

    // 测试共轭梯度法
    {
        matxd A(4, 4);
        A(0, 0) = 4, A(0, 1) = -2, A(0, 2) = 4, A(0, 3) = 2;
        A(1, 0) = -2, A(1, 1) = 10, A(1, 2) = -7, A(1, 3) = -2;
        A(2, 0) = 4, A(2, 1) = -7, A(2, 2) = 8, A(2, 3) = 4;
        A(3, 0) = 2, A(3, 1) = -2, A(3, 2) = 4, A(3, 3) = 7;
        vecxd b{8, -1, 14, 6};

        auto x = conjugate_gradient(A, b);
        EXPECT_LT((A * x - b).norm(), 1e-6);
    }

    // 测试二分法
    {
        diagmat3d A(4);
        A(0, 0) = 1, A(0, 1) = 1;
        A(1, 0) = 1, A(1, 1) = 2, A(1, 2) = 3;
        A(2, 1) = 3, A(2, 2) = 6, A(2, 3) = 10;
        A(3, 2) = 10, A(3, 3) = 20;

        auto lambda = sym_eigenvalues(A);
        EXPECT_LT(
            (lambda - vecxd({-1.523734777993481, 0.974969844946525, 4.258610702322208, 25.290154230724745})).norm(),
            1e-6);
    }
}